/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "fc_sensors.h"
#include "imu.h"
#include "fc.h"
#include "nds03_api.h"
#define TOF_RANGE_MAX 2500

#define CONVERIANT_SAMPLE 100
int16_t cov_buffer[CONVERIANT_SAMPLE];
int16_t cov_index = 0;
int16_t cov_sum = 0;

// void sensor_tof_init(void)
// {
// }

// void sensor_tof_handle(struct rt_sensor_data *sensor)
// {
//     if(sensor->type == RT_SENSOR_CLASS_TOF)
//     {
//         tof_t *tof = &fc_sensor_data.tof;
//         tof->range = sensor->data.tof.depth;//0-2500
//         tof->quality = sensor->data.tof.confi;//0-100
//     }
// }

void tof_update(void)
{
    uint16_t range, confi;
    int ret = -1;

#if USED_TOF
    ret = nds03_get_result_data(&range, &confi);
#endif

    if (ret == 0)
    {
        fc_sensor_data.tof.range = range;
        fc_sensor_data.tof.quality = confi;
        fc_sensor_data.tof.isOnline = 1;
    }
    else
    {
        fc_sensor_data.tof.isOnline = 0;
        fc_sensor_data.tof.quality = 0;
        fc_sensor_data.tof.range = 0;
    }
    // rt_kprintf("TOF: %d %d %d\n", ret, range, confi);
}
